#include <os/fifobuff.h>
#include <os/fifoio.h>
#include <os/safety.h>
#include <os/debug.h>
#include <os/memcache.h>
#include <arch/memory.h>
#include <lib/math.h>
#include <lib/string.h>

// fifo buffer opertor function declare
void FifoBuffInit(fifobuff_t *fifobuff, void *buff, uint32_t size)
{
    fifobuff->buffer = buff;
    fifobuff->size = size;
    fifobuff->in = 0;
    fifobuff->out = 0;
    SpinLockInit(&fifobuff->lock);
}

fifobuff_t *FifoBuffAlloc(uint32_t size)
{
    void *buffer;
    fifobuff_t *fifobuff;

    if (!IS_POWER_OF_2(size))
    {
        size = roundup_pow_of_two(size);
    }
    // alloc buffer
    buffer = KMemAlloc(size);
    if (!buffer)
        return NULL;
    // alloc fifo buffer object
    fifobuff = KMemAlloc(sizeof(fifobuff_t));
    if (!fifobuff)
    {
        KMemFree(buffer);
        return NULL;
    }
    // init fifo buffer
    FifoBuffInit(fifobuff, buffer, size);
    return fifobuff;
}

void FifoBuffFree(fifobuff_t *fifobuff)
{
    KMemFree(fifobuff->buffer);
    KMemFree(fifobuff);
}

uint32_t _FifoBuffPut(fifobuff_t *fifobuff, uint8_t *buff, uint32_t len)
{
    uint32_t length;

    // figure buffer free length
    len = MIN(len, fifobuff->size - fifobuff->in + fifobuff->out);
    // ensure index same
    mb();
    // first write from in to out
    length = MIN(len, fifobuff->size - (fifobuff->in & (fifobuff->size - 1)));
    // first copy data to fifobuff->in end
    memcpy((uint8_t *)fifobuff->buffer + (fifobuff->in & (fifobuff->size - 1)), buff, length);
    // then copy rest to fifobuff before
    memcpy(fifobuff->buffer, buff + length, len - length);
    // ensure data writes
    wmb();
    // update fifobuff->in index when buffer put end
    fifobuff->in += len;
    return len;
}

uint32_t _FifoBuffGet(fifobuff_t *fifobuff, uint8_t *buff, uint32_t len)
{
    uint32_t length;

    // figure buffer  data length
    len = MIN(len, fifobuff->in - fifobuff->out);
    // figure fifobuffer data length
    length = MIN(len, fifobuff->size - (fifobuff->out & (fifobuff->size - 1)));
    // first copy data from fifobuff until the end of buffer
    memcpy(buff, fifobuff->buffer + (fifobuff->out & (fifobuff->size - 1)), length);
    // then copy rest to buffer before
    memcpy(buff + length, fifobuff->buffer, len - length);
    // update fifobuff->out index when fifobuff get end
    fifobuff->out += len;
    /*KPrint("fifo get: ");
    for (int i = 0; i < length; i++)
    {
        KPrint("%d ", (uint32_t)buff[i]);
    }*/
    KPrint("\n");
    return len;
}
